#include "RSIM_INC.h"

#include <cstdlib>
#include <iomanip>
#include <fstream>

using namespace std;
using namespace RSIM;

int main(){                

        double mass = 1.0;
        Mat3x3 I;
        I.Diagonal(1.0,2.0,3.0);        

        MultibodySystem MBS1,MBS2,MBS3, MBS4;
        RigidBodyNode *tmp1, *tmp2, *tmp3;
           
        
        tmp1 = MBS1.setRoot(RigidBody("Pin",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(0,0,0))));        
        tmp2 = MBS1.addNodeTo(tmp1, RigidBody("Pin",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(1.5,0,0))));
        tmp3 = MBS1.addNodeTo(tmp2, RigidBody("Pin",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(1.5,0,0))));	
        MBS1.setTopologyMaps();
        State state1(MBS1);                
        
        tmp1 = MBS2.setRoot(RigidBody("Pin",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(0,0,0))));        
        tmp2 = MBS2.addNodeTo(tmp1, RigidBody("Pin",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(1.5,0,0))));
        tmp3 = MBS2.addNodeTo(tmp2, RigidBody("Pin",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(1.5,0,0))));    
        MBS2.setTopologyMaps();
        State state2(MBS2);        
        
        SystemData SD;
        
        SD.addSystemData(&MBS1,&state1);
	SD.addSystemData(&MBS2,&state2);
        SD.addGravityLikeAcceleration(Vect3(0.0,-9.81,0.0));        
        SD.finalize();                                        
        MBS1.initEOM();
        MBS1.initABA();
        
        MBS2.initEOM();
        MBS2.initABA();
                
        
        fstream opFile;
        opFile.open("y",ios::out | ios::app);
        
        Integrator *Integ = new Ode45;
        Integ->addReporter(new StateReporter(SD,0.1,opFile));
        Integ->stepTo(10.0,SD);
        opFile.close();
        delete Integ;        
        return 0;
}
